#include <math.h>
#include <string.h>
#include "pm_std.h"
#include "sm_std.h"
#include "ne_std.h"
#include "ne_dae.h"
#include "sm_ssci_run_time_errors.h"
#include "sm_RuntimeDerivedValuesBundle.h"
#include "openManipulatorTrajectoryPlanning_6a081443_1_geometries.h"
PmfMessageId openManipulatorTrajectoryPlanning_6a081443_1_compOutputs ( const
RuntimeDerivedValuesBundle * rtdv , const double * state , const int *
modeVector , const double * input , const double * inputDot , const double *
inputDdot , const double * discreteState , double * output ,
NeuDiagnosticManager * neDiagMgr ) { const double * rtdvd = rtdv -> mDoubles
. mValues ; const int * rtdvi = rtdv -> mInts . mValues ; double xx [ 49 ] ;
( void ) rtdvd ; ( void ) rtdvi ; ( void ) modeVector ; ( void ) input ; (
void ) inputDot ; ( void ) inputDdot ; ( void ) discreteState ; ( void )
neDiagMgr ; xx [ 0 ] = - 0.9952875672845549 ; xx [ 1 ] = -
0.09696730578610657 ; xx [ 2 ] = xx [ 0 ] ; xx [ 3 ] = - 7.396429069217447e-8
; xx [ 4 ] = xx [ 1 ] ; xx [ 5 ] = 4.167847961547566e-6 ; xx [ 6 ] = -
0.9953936295809525 ; xx [ 7 ] = - 5.058992580434195e-4 ; xx [ 8 ] =
0.09584258966610114 ; xx [ 9 ] = 2.337576029516223e-3 ; xx [ 10 ] = 0.5 ; xx
[ 11 ] = xx [ 10 ] * state [ 6 ] ; xx [ 12 ] = 8.310758752905212e-6 ; xx [ 13
] = sin ( xx [ 11 ] ) ; xx [ 14 ] = 0.9999999999652471 ; xx [ 15 ] =
6.610584974975353e-7 ; xx [ 16 ] = cos ( xx [ 11 ] ) ; xx [ 17 ] = xx [ 12 ]
* xx [ 13 ] ; xx [ 18 ] = xx [ 14 ] * xx [ 13 ] ; xx [ 19 ] = - ( xx [ 15 ] *
xx [ 13 ] ) ; pm_math_Quaternion_compose_ra ( xx + 6 , xx + 16 , xx + 20 ) ;
xx [ 6 ] = - 0.9984606559510578 ; xx [ 7 ] = - 1.525595350473025e-3 ; xx [ 8
] = 0.05467563387767577 ; xx [ 9 ] = - 9.195984813180454e-3 ; xx [ 11 ] = xx
[ 10 ] * state [ 4 ] ; xx [ 13 ] = 4.564927305666077e-3 ; xx [ 16 ] = sin (
xx [ 11 ] ) ; xx [ 17 ] = 0.9999885204623049 ; xx [ 18 ] =
1.456153255698589e-3 ; xx [ 24 ] = cos ( xx [ 11 ] ) ; xx [ 25 ] = xx [ 13 ]
* xx [ 16 ] ; xx [ 26 ] = xx [ 17 ] * xx [ 16 ] ; xx [ 27 ] = xx [ 18 ] * xx
[ 16 ] ; pm_math_Quaternion_compose_ra ( xx + 6 , xx + 24 , xx + 28 ) ; xx [
6 ] = - 0.9984344664497182 ; xx [ 7 ] = - 2.745981335367077e-3 ; xx [ 8 ] = -
0.05574004796882191 ; xx [ 9 ] = 3.758037281069619e-3 ; xx [ 11 ] = xx [ 10 ]
* state [ 2 ] ; xx [ 10 ] = 0.01415235514121426 ; xx [ 16 ] = sin ( xx [ 11 ]
) ; xx [ 19 ] = 0.9998919020591995 ; xx [ 24 ] = 3.986858461632603e-3 ; xx [
32 ] = cos ( xx [ 11 ] ) ; xx [ 33 ] = - ( xx [ 10 ] * xx [ 16 ] ) ; xx [ 34
] = xx [ 19 ] * xx [ 16 ] ; xx [ 35 ] = xx [ 24 ] * xx [ 16 ] ;
pm_math_Quaternion_compose_ra ( xx + 6 , xx + 32 , xx + 36 ) ; xx [ 6 ] =
4.385038631106116e-3 * state [ 1 ] ; xx [ 7 ] = - ( 0.01057602111633074 *
state [ 1 ] ) ; xx [ 8 ] = 0.999934457458863 * state [ 1 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 36 , xx + 6 , xx + 25 ) ; xx [ 32 ]
= xx [ 25 ] - xx [ 10 ] * state [ 3 ] ; xx [ 33 ] = xx [ 26 ] + xx [ 19 ] *
state [ 3 ] ; xx [ 34 ] = xx [ 27 ] + xx [ 24 ] * state [ 3 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 28 , xx + 32 , xx + 9 ) ; xx [ 24 ]
= xx [ 9 ] + xx [ 13 ] * state [ 5 ] ; xx [ 25 ] = xx [ 10 ] + xx [ 17 ] *
state [ 5 ] ; xx [ 26 ] = xx [ 11 ] + xx [ 18 ] * state [ 5 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 20 , xx + 24 , xx + 9 ) ; xx [ 16 ]
= xx [ 9 ] + xx [ 12 ] * state [ 7 ] ; xx [ 17 ] = xx [ 10 ] + xx [ 14 ] *
state [ 7 ] ; xx [ 18 ] = xx [ 11 ] - xx [ 15 ] * state [ 7 ] ; xx [ 9 ] =
0.05259203501857801 ; xx [ 10 ] = xx [ 12 ] * state [ 10 ] ; xx [ 11 ] = xx [
14 ] * state [ 10 ] ; xx [ 12 ] = 4.439189670580984e-7 ; xx [ 13 ] =
0.01034590993618254 ; xx [ 19 ] = xx [ 15 ] * state [ 10 ] ; xx [ 40 ] = xx [
9 ] + xx [ 10 ] ; xx [ 41 ] = xx [ 11 ] - xx [ 12 ] ; xx [ 42 ] = - ( xx [ 13
] + xx [ 19 ] ) ; pm_math_Vector3_cross_ra ( xx + 16 , xx + 40 , xx + 43 ) ;
xx [ 40 ] = 0.1239983924299681 ; xx [ 41 ] = - 5.656415752171374e-4 ; xx [ 42
] = - 2.805786735064276e-4 ; pm_math_Vector3_cross_ra ( xx + 24 , xx + 40 ,
xx + 46 ) ; xx [ 24 ] = 0.01017880053681747 ; xx [ 25 ] = -
3.736067231021393e-4 ; xx [ 26 ] = 0.1298316311137164 ;
pm_math_Vector3_cross_ra ( xx + 32 , xx + 24 , xx + 40 ) ; xx [ 24 ] =
2.543322406041548e-4 ; xx [ 25 ] = - 6.134092247471828e-4 ; xx [ 26 ] =
0.05799619853261406 ; pm_math_Vector3_cross_ra ( xx + 6 , xx + 24 , xx + 32 )
; pm_math_Quaternion_inverseXform_ra ( xx + 36 , xx + 32 , xx + 6 ) ; xx [ 24
] = xx [ 40 ] + xx [ 6 ] ; xx [ 25 ] = xx [ 41 ] + xx [ 7 ] ; xx [ 26 ] = xx
[ 42 ] + xx [ 8 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 28 , xx + 24 ,
xx + 6 ) ; xx [ 24 ] = xx [ 46 ] + xx [ 6 ] ; xx [ 25 ] = xx [ 47 ] + xx [ 7
] ; xx [ 26 ] = xx [ 48 ] + xx [ 8 ] ; pm_math_Quaternion_inverseXform_ra (
xx + 20 , xx + 24 , xx + 6 ) ; xx [ 20 ] = xx [ 43 ] + xx [ 6 ] ; xx [ 21 ] =
xx [ 44 ] + xx [ 7 ] ; xx [ 22 ] = xx [ 45 ] + xx [ 8 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 2 , xx + 20 , xx + 23 ) ; xx [ 2 ]
= 2.0 ; xx [ 3 ] = 0.7071067811865476 ; xx [ 4 ] = xx [ 24 ] + state [ 11 ] ;
xx [ 5 ] = xx [ 3 ] * xx [ 4 ] * xx [ 3 ] ; xx [ 15 ] = xx [ 3 ] * xx [ 3 ] *
xx [ 25 ] ; xx [ 20 ] = xx [ 25 ] + xx [ 2 ] * ( xx [ 5 ] - xx [ 15 ] ) ; xx
[ 21 ] = 5.464378949326942e-17 ; xx [ 22 ] = 1.387778780781446e-17 ; xx [ 26
] = - xx [ 21 ] ; xx [ 27 ] = xx [ 22 ] ; xx [ 28 ] = 6.938893903907228e-18 ;
xx [ 29 ] = xx [ 23 ] ; xx [ 30 ] = xx [ 4 ] - ( xx [ 15 ] + xx [ 5 ] ) * xx
[ 2 ] ; xx [ 31 ] = xx [ 20 ] ; pm_math_Vector3_cross_ra ( xx + 26 , xx + 29
, xx + 23 ) ; pm_math_Vector3_cross_ra ( xx + 26 , xx + 23 , xx + 29 ) ; xx [
26 ] = 0.7037746403582225 ; xx [ 27 ] = - 0.7037745357569194 ; xx [ 28 ] =
0.06856918658830206 ; xx [ 29 ] = 0.06856329236118894 ; xx [ 32 ] = xx [ 9 ]
; xx [ 33 ] = - xx [ 12 ] ; xx [ 34 ] = - xx [ 13 ] ;
pm_math_Vector3_cross_ra ( xx + 16 , xx + 32 , xx + 35 ) ; xx [ 32 ] = xx [ 6
] + xx [ 35 ] ; xx [ 33 ] = xx [ 7 ] + xx [ 36 ] ; xx [ 34 ] = xx [ 8 ] + xx
[ 37 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 26 , xx + 32 , xx + 35 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 26 , xx + 16 , xx + 38 ) ; xx [ 41
] = xx [ 10 ] ; xx [ 42 ] = xx [ 11 ] ; xx [ 43 ] = - xx [ 19 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 26 , xx + 41 , xx + 44 ) ;
pm_math_Vector3_cross_ra ( xx + 38 , xx + 44 , xx + 26 ) ; xx [ 38 ] = xx [ 0
] ; xx [ 39 ] = - 7.396429119177483e-8 ; xx [ 40 ] = xx [ 1 ] ; xx [ 41 ] =
4.167847961603077e-6 ; xx [ 0 ] = 8.310758753113379e-6 * state [ 8 ] ; xx [ 1
] = xx [ 14 ] * state [ 8 ] ; xx [ 4 ] = 6.610584966093569e-7 * state [ 8 ] ;
xx [ 42 ] = xx [ 9 ] - xx [ 0 ] ; xx [ 43 ] = - ( xx [ 12 ] + xx [ 1 ] ) ; xx
[ 44 ] = xx [ 4 ] - xx [ 13 ] ; pm_math_Vector3_cross_ra ( xx + 16 , xx + 42
, xx + 9 ) ; xx [ 12 ] = xx [ 9 ] + xx [ 6 ] ; xx [ 13 ] = xx [ 10 ] + xx [ 7
] ; xx [ 14 ] = xx [ 11 ] + xx [ 8 ] ; pm_math_Quaternion_inverseXform_ra (
xx + 38 , xx + 12 , xx + 5 ) ; xx [ 8 ] = xx [ 6 ] - state [ 9 ] ; xx [ 9 ] =
xx [ 3 ] * xx [ 3 ] * xx [ 8 ] ; xx [ 10 ] = xx [ 3 ] * xx [ 3 ] * xx [ 7 ] ;
xx [ 3 ] = xx [ 7 ] - xx [ 2 ] * ( xx [ 9 ] + xx [ 10 ] ) ; xx [ 11 ] =
0.7037745357569191 ; xx [ 12 ] = 0.7037746403582228 ; xx [ 13 ] =
0.0685632923611889 ; xx [ 14 ] = - 0.06856918658830211 ;
pm_math_Quaternion_inverseXform_ra ( xx + 11 , xx + 32 , xx + 38 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 11 , xx + 16 , xx + 32 ) ; xx [ 15
] = - xx [ 0 ] ; xx [ 16 ] = - xx [ 1 ] ; xx [ 17 ] = xx [ 4 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 11 , xx + 15 , xx + 41 ) ;
pm_math_Vector3_cross_ra ( xx + 32 , xx + 41 , xx + 11 ) ; output [ 0 ] =
state [ 8 ] ; output [ 1 ] = state [ 10 ] ; output [ 2 ] = state [ 0 ] ;
output [ 3 ] = state [ 2 ] ; output [ 4 ] = state [ 4 ] ; output [ 5 ] =
state [ 6 ] ; output [ 6 ] = xx [ 20 ] + ( xx [ 25 ] + xx [ 31 ] ) * xx [ 2 ]
- xx [ 37 ] - xx [ 28 ] ; output [ 7 ] = xx [ 3 ] + xx [ 2 ] * ( 1.0 * ( xx [
21 ] * ( xx [ 8 ] - ( xx [ 9 ] - xx [ 10 ] ) * xx [ 2 ] ) - xx [ 22 ] * xx [
5 ] ) - ( xx [ 3 ] * 2.985943730184742e-33 + xx [ 3 ] * 1.925929944387236e-34
) ) - xx [ 40 ] - xx [ 13 ] ; return NULL ; }
